#!/usr/bin/env python

# -*- coding: utf-8 -*

"""
车子驱动，包含4个轮子和两个摄像头舵机
"""

from py_code.driver.Driver import Driver
from py_code.SingletonType import SingletonType
from py_code.driver.YB_Pcb_Car import YB_Pcb_Car
import logging

class CarDriver(Driver, metaclass = SingletonType):
    ybCar: YB_Pcb_Car = None
    servoAngle = [None, None, None, None, None]

    def __init__(self):
        super().__init__()
        self.ybCar = YB_Pcb_Car()
        logging.debug(f"车子协处理器驱动 构造函数")

    def run(self, left, right):
        """
        车子行走
        @param left 左侧轮子速度 -180~180
        @param right 右侧轮子速度 -180~180
        """
        self.ybCar.Control_Car(left, right)
        logging.debug(f"车子行走，左边速度：{left}，右边速度：{right}")

    def go(self, speed):
        """
        车子前进或者后退（不拐弯）
        @param speed 速度 -180~180
        """
        self.run(speed, speed)

    def stop(self):
        self.ybCar.Car_Stop()
        logging.debug(f"车子停止")

    def servo(self, id, angle):
        """
        舵机
        @param id 舵机编号，1~4
        @param angle 角度，0-180
        """
        self.ybCar.Ctrl_Servo(id, angle)
        self.servoAngle[id] = angle
        logging.debug(f"舵机转动，舵机编号：{id}，角度：{angle}")

    def cameraRightLeft(self, angle):
        """
        摄像头舵机左右转动
        @param angle 从右到左的角度 0-180
        """
        self.servo(1, angle)

    def cameraUpDown(self, angle):
        """
        摄像头舵机上下转动
        @param angle 从上到下的角度 0-180
        """
        self.servo(2, angle)

    def close(self):
        self.stop()
        super().close()

    def reset(self):
        self.stop()
        super().reset()
    